//############################################################
// FILE: Task_function.c
// Created on: 2017年1月18日
// Author: XQ
// summary: Task_function
//本程序只供学习使用，未经作者许可，不得用于其它任何用途
//版权所有，盗版必究
//DSP/STM32电机控制开发板
//硕历电子
//网址: https://shuolidianzi.taobao.com
//Author-QQ: 616264123
//电机控制QQ群：314306105
//############################################################
#include "main_user.h"

#include "FOC_run.h"
#include "RS485.h"
#include "Timer.h"   
#include "Axis_transform.h"   
#include "ThreeHall.h"  
#include "Svpwm_dq.h"
#include "IQ_math.h"
#include "PI_Cale.h"
#include "Task_function.h"

#include "SEGGER_RTT_Port.h"

void DWT_Init(void);
void Offset_CurrentReading(void);
void Uart_485_Init(void);

PI_Control   pi_spd = PI_Control_DEFAULTS;
PI_Control   pi_id  = PI_Control_DEFAULTS;
PI_Control   pi_iq  = PI_Control_DEFAULTS;
 
Test         TestPare = Test_DEFAULTS;
TaskTime     TaskTimePare = TaskTime_DEFAULTS;
logic        logicContr = logic_DEFAULTS;

//Hall         Hall_Three = Hall_DEFAULTS;
CLARKE       ClarkeI = CLARKE_DEFAULTS;
PARK         ParkI = PARK_DEFAULTS;
IPARK        IparkU = IPARK_DEFAULTS;
SVPWM        Svpwmdq = SVPWM_DEFAULTS;
IQSin_Cos    AngleSin_Cos = IQSin_Cos_DEFAULTS ;
IQAtan       IQAtan_Pare = IQAtan_DEFAULTS;
//CANSR        CANSRPare = CANSR_DEFAULTS;



void main_user(void)
{
	DWT_Init();    //调试计数器
	
	//ADC校准
	HAL_ADCEx_Calibration_Start( &hadc1);
	HAL_ADCEx_Calibration_Start( &hadc2);
	__HAL_ADC_CLEAR_FLAG( &hadc1, ADC_FLAG_JEOC);
	__HAL_ADC_CLEAR_FLAG( &hadc2, ADC_FLAG_JEOC);
	//启动ADC1规则组的DMA连续转换
  HAL_ADC_Start_DMA(&hadc1, (uint32_t*)ADC_ConvertedValue, 5);
	HAL_Delay(10);
  Offset_CurrentReading();  //校准作用,电流传感器的理论偏移值为1.65V
	
	__HAL_TIM_CLEAR_FLAG( &htim1, TIM_FLAG_BREAK);
	HAL_TIM_PWM_Start( &htim1, TIM_CHANNEL_4);
  HAL_TIM_Base_Start_IT(&htim1);  // 启动定时器溢出中断
		
	//__HAL_TIM_CLEAR_FLAG( &htim4, TIM_FLAG_BREAK);
	//HAL_TIM_Encoder_Start( &htim4, TIM_CHANNEL_ALL);


  //485初始化
	Uart_485_Init();
	RS485_StartReceive();
	__HAL_UART_ENABLE_IT( &huart3, UART_IT_IDLE);
	__HAL_UART_CLEAR_IDLEFLAG( &huart3);
	__HAL_UART_DISABLE_IT( &huart3, UART_IT_TC);
	__HAL_UART_CLEAR_FLAG( &huart3, UART_FLAG_TC);

	HAL_Delay(100);
	
	
	/******* 用户区初始化  **********/
	//F1_Delay_Init();
	
	ThreeHallPara_init( );// 三霍尔角度传感器的参数初始化
	PI_Pare_init( );           // 三个双PID参数初始化	
	
	logicContr.Run_mode = 5;	    // 开环闭环正反转模式
	
	HAL_ADCEx_InjectedStart_IT(&hadc1);  //开启FOC运行
	HAL_ADCEx_InjectedStart(&hadc2);	


 // SEGGER_RTT_Port_Init();
 
//	 set_motor_pid(
//      3.5, 0, 7,
//      0.02, 0.001, 0,
//      1570, 30, 0,
//      1570, 30, 0);

//	//校准传感器与电机零点偏移角度
//  set_pwm_duty(0.5, 0, 0);              // d轴强拖，形成SVPWM模型中的基础矢量1，即对应转子零度位置
//  HAL_Delay(400);                       // 保持一会
//  rotor_zero_angle = encoder_angle;
//  set_pwm_duty(0, 0, 0);                // 松开电机
//  HAL_Delay(100);

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
	uint32_t previousMillis = 0;
  const uint32_t interval = 1000; // 1000 ms 间隔
	
	for(int i = 0;i < 55;i++)
	{
//		__disable_irq();
////		  Hall_Three.Initial_eleangle += 2500;
////		  init_angle += 2500;
//		__enable_irq();
//		pi_id.Ref=0;
//		HAL_Delay(5000);
//		pi_id.Ref=500;
//		HAL_Delay(10000);
	}
			 
  while (1)
  {
		 // 获取当前的系统时间
		 uint32_t currentMillis = HAL_GetTick();

		 // 检查是否已达到间隔时间
		 if (currentMillis - previousMillis >= interval) {
				previousMillis = currentMillis;  // 更新上次执行的时间
			 
			 
        // 调用要定时执行的函数
			 //disable_erq();   float FOC_time_us = time_us; void (* FOC_func_ptr)(ADC_HandleTypeDef *hadc) = func_ptr;enable_erq();
//			  myprintf("Function execution time: %.3f us\nFunction %p is in %s memory\n",\
//				FOC_time_us,FOC_func_ptr,((uint32_t)func_ptr >= SRAM_BASE) ? "RAM" : "Flash"); 
			 
			  //HAL_IWDG_Refresh(&hiwdg);  // 喂狗，重装载计数器
			// UploadData();	

		 }
		
  }
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM4)       //判断是否为定时器4产生的中断
    {
		  //T_s_overflag = 1 ;
    }
}


/**********************  DWT  ************************/
void DWT_Init(void) {
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; // 使能 DWT
    DWT->CYCCNT = 0; // 计数器清零
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; // 使能 CYCCNT 计数器
}


/**********************  ADC  ************************/
ADCSamp      ADCSampPare = ADCSamp_DEFAULTS;
uint16_t  ADC_ConvertedValue[5]={0};
uint32_t adc1_in0, adc1_in1, adc2_in2, adc1_in8, adc1_in3;
//校准作用,电流传感器的理论偏移值为1.65V
//三电阻采样时可以用软件构建动态的偏移电压的ADC值，三相电流的ADC值相加是3倍的偏移电压ADC值的数学关系
void Offset_CurrentReading(void)
{
	static uint8_t i;  
 
  /* ADC Channel used for current reading are read  in order to get zero currents ADC values*/
  //64次采样求平均值，电流传感器初始校准   
for(i=64; i>0; i--)   
  {
   //对于相电流和母线电流的电阻法测量电流,需要上电读取初始实际偏执电压，这个电压大概为1.65V左右，我们将该电压值作为电流为0的电压值
    ADCSampPare.OffsetBUS_Curr += ADC_ConvertedValue[0];
 	  ADCSampPare.OffsetPhaseV_Curr += ADC_ConvertedValue[1];
    ADCSampPare.OffsetPhaseU_Curr += ADC_ConvertedValue[2];		   
    HAL_Delay(2);
  }
  ADCSampPare.OffsetBUS_Curr = ADCSampPare.OffsetBUS_Curr>>6;  //求64次电压信号平均值
	ADCSampPare.OffsetPhaseV_Curr= ADCSampPare.OffsetPhaseV_Curr>>6; 
	ADCSampPare.OffsetPhaseU_Curr=ADCSampPare.OffsetPhaseU_Curr>>6;
}


void ADC_Sample(void)
{
	adc1_in1 = HAL_ADCEx_InjectedGetValue( &hadc1, ADC_INJECTED_RANK_1);
	adc1_in0 = HAL_ADCEx_InjectedGetValue( &hadc1, ADC_INJECTED_RANK_2);
	adc1_in8 = HAL_ADCEx_InjectedGetValue( &hadc1, ADC_INJECTED_RANK_3); //ADC_ConvertedValue[4];
	adc1_in3 = HAL_ADCEx_InjectedGetValue( &hadc1, ADC_INJECTED_RANK_4); //ADC_ConvertedValue[3];
	adc2_in2  = HAL_ADCEx_InjectedGetValue( &hadc2, ADC_INJECTED_RANK_1);
	
	/*Uoffset - IV-V = 0.2IV-I   偏移电压减去测量的电压等于0.2倍该路电流,FOC 中通常定义流入电机绕组的电流为正（符合右手定则）
	  低端采样时，相电流流经采样电阻进入PGND（下桥臂导通）时，该电流方向通常是“流出电机”，在 FOC 定义中应为负值,
	  因此这里在计算后的电流前加一个负号
	
	  如果流入电机的相电流定义为正，则 Iq > 0 时，电机通常逆时针（CCW）旋转。
    Iq < 0 时，电机通常顺时针（CW）旋转。
	*/
	//这里将电流作为Q6.10格式，电流理论值为(adcx_inx-ADCSampPare.Offsetxx)*3.3/4095*5 ≈ (adcx_inx-ADCSampPare.Offsetxx)/2^8
	//我们只需要将电流值赋为(adcx_inx-ADCSampPare.Offsetxx)再左移2位即可转换为Q6.10格式定点数，+-32A符合要求
  ADCSampPare.BUS_Curr  =     - (ADCSampPare.OffsetBUS_Curr - adc1_in0)<<2; //(adc1_in0-ADCSampPare.OffsetBUS_Curr)<<2;	     // 
 	ADCSampPare.PhaseV_Curr  =  - (ADCSampPare.OffsetPhaseV_Curr - adc1_in1)<<2;	  //(adc1_in1-ADCSampPare.OffsetPhaseV_Curr)<<2;	   // 
  ADCSampPare.PhaseU_Curr  =  - (ADCSampPare.OffsetPhaseU_Curr - adc2_in2)<<2;	//(adc2_in2-ADCSampPare.OffsetPhaseU_Curr)<<2;	   //
  ADCSampPare.RP_speed_Voltage = adc1_in3;	       // 实际值 = ADCSampPare.RP_speed_Voltage/4096*3.3
  ADCSampPare.BUS_Voltage   = adc1_in8;	         // 实际值 = ADCSampPare.BUS_Voltage/4096*21*3.3
 
	//下式右边的ADCSampPare.BUS_CurrFitter为c(t-1)，左边的ADCSampPare.BUS_CurrFitter为c(t)，Speed_count为r(t)，T为采样时间0.1ms，RC根据截止频率人为设定 */
	ADCSampPare.BUS_CurrFitter =  _IQ10mpy(ADCSampPare.Coeff_filterK1, ADCSampPare.BUS_CurrFitter)+_IQ10mpy(ADCSampPare.Coeff_filterK2, ADCSampPare.BUS_Curr);    
	ADCSampPare.PhaseU_CurrFitter =  _IQ10mpy(ADCSampPare.Coeff_filterK1, ADCSampPare.PhaseU_CurrFitter)+_IQ10mpy(ADCSampPare.Coeff_filterK2, ADCSampPare.PhaseU_Curr);   
	ADCSampPare.PhaseV_CurrFitter =  _IQ10mpy(ADCSampPare.Coeff_filterK1, ADCSampPare.PhaseV_CurrFitter)+_IQ10mpy(ADCSampPare.Coeff_filterK2, ADCSampPare.PhaseV_Curr);   
}


/**********************  UART  ************************/
uint8_t aRxBuffer[100];

void Uart_485_Init(void){
   HAL_GPIO_WritePin(REN485_LED_GPIO_Port, REN485_LED_Pin, GPIO_PIN_RESET);
	 HAL_UARTEx_ReceiveToIdle_DMA(&huart3, aRxBuffer, sizeof(aRxBuffer));  //��������3��DMA�����ж�
	 __HAL_DMA_DISABLE_IT(&hdma_usart3_rx, DMA_IT_HT);  //ȡ���������ݹ����ж�
}
//===========================================================================
// No more.
//===========================================================================
